/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#include <base_local_planner/line_iterator.h>
#include <base_local_planner/costmap_model.h>
#include <costmap_2d/cost_values.h>

using namespace std;
using namespace costmap_2d;

/**
 * @brief CostmapModel类继承了WorldModel，能够获取点、连线、多边形轮廓的cost，是局部规划器与costmap间的一个桥梁。初始化在move_base的构造函数
 * @brief 使用方法如下：
  costmap_2d::Costmap2DROS*  planner_costmap_ros_
  world_model_ = new base_local_planner::CostmapModel(*(planner_costmap_ros_ ->getCostmap()) ); 
  std::vector<geometry_msgs::Point> footprint = planner_costmap_ros_->getRobotFootprint();
  double footprint_cost = world_model_->footprintCost(x_i, y_i, theta_i, footprint);

  CostmapModel类帮助local planner在Costmap上进行计算, footprintCost, lineCost, pointCost三个类方法分别能通过Costmap计算出机器人足迹范围、
  两个cell连线、单个cell的代价，并将值返回给局部规划器。
 */

namespace base_local_planner {
  CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}

  // footprintCost函数是对footprint中的每个线段进行独立计算代价的。首先尝试获取线段的地图坐标，然后利用lineCost函数计算线段代价值，并用footprint_cost
  // 参数保存最大的线段代价值，最终如果正常，则返回的也是footprint_cost。只要有1个线段的代价值小于0，则直接返回-1。
  // 返回值：
  // -1.0 ：覆盖至少一个障碍cell    没有撞障碍时，返回值是 0。撞障碍基本都是 -1
  // -2.0 ：覆盖至少一个未知cell
  // -3.0 ：不在地图上
  // 其他: a positive value for traversable space

  //参数：函数原型对第一个参数position的解释是：The position of the robot in world coordinates。实际是 局部代价地图的全局坐标系 ，一般是map或odom
  // https://charon-cheung.github.io/2020/11/15/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/CostmapModel/#footprintCost
  double CostmapModel::footprintCost(const geometry_msgs::Point& position, 
                       const std::vector<geometry_msgs::Point>& footprint,
                                                  double inscribed_radius, 
                                                  double circumscribed_radius){
    unsigned int cell_x, cell_y;
    if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
      return -3.0;
    // 把足迹视为多边形，循环调用 lineCost 计算多边形各边的cell，注意首尾闭合，最后返回代价。 对于圆形底盘，按16边形处理
    if(footprint.size() < 3){         //if number of points in the footprint is less than 3, we'll just assume a circular robot
      unsigned char cost = costmap_.getCost(cell_x, cell_y);
      if(cost == NO_INFORMATION)
        return -2.0;
      if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
        return -1.0;
      return cost;
    }

    //now we really have to lay down the footprint in the costmap grid
    unsigned int x0, x1, y0, y1;
    double line_cost = 0.0;
    double footprint_cost = 0.0;

    //我们需要栅格化 footprint 中的每条线
    for(unsigned int i = 0; i < footprint.size() - 1; ++i){
      //获取第一个点的单元格坐标
      if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
        return -3.0;

      //get the cell coord of the second point
      if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
        return -3.0;

      line_cost = lineCost(x0, x1, y0, y1);
      footprint_cost = std::max(line_cost, footprint_cost);

      //if there is an obstacle that hits the line... we know that we can return false right away
      if(line_cost < 0)
        return line_cost;
    }

    // 以下是连接 footprint 的第一个和最后一个点
    //get the cell coord of the last point
    if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
      return -3.0;

    //get the cell coord of the first point
    if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
      return -3.0;

    // 搜索发现有3个`lineCost`函数, `TrajectoryPlanner::lineCost`, `VoxelGridModel::lineCost`, 
    // `CostmapModel::lineCost`。经过运行， 发现调用的还是CostmapModel::lineCost 
    line_cost = lineCost(x0, x1, y0, y1);
    footprint_cost = std::max(line_cost, footprint_cost);

    if(line_cost < 0)
      return line_cost;

    //if all line costs are legal... then we can return that the footprint is legal
    return footprint_cost;

  }

  // 对于线段的代价计算函数lineCost，其实是对线段中的所有像素点（通过bresenham算法得到，不必深入研究）进行代价计算，
  // 如果像素点的代价为LETHAL_OBSTACLE或者NO_INFORMATION，则该点代价为-1。如果某个点的代价为-1，则该线段的代价也为-1，
  // 否则取所有点中最大的代价作为线段的代价
  //calculate the cost of a ray-traced line
  double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const {
    double line_cost = 0.0;
    double point_cost = -1.0;

    for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() ){
      point_cost = pointCost( line.getX(), line.getY() ); //Score the current point

      if(point_cost < 0)
        return point_cost;

      if(line_cost < point_cost)
        line_cost = point_cost;
    }

    return line_cost;
  }

  double CostmapModel::pointCost(int x, int y) const {
    unsigned char cost = costmap_.getCost(x, y);
    //if the cell is in an obstacle the path is invalid
    if(cost == NO_INFORMATION)
      return -2;
    if(cost == LETHAL_OBSTACLE)
      return -1;

    return cost;
  }

};
